/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    Joystick leftStick;
    Joystick rightStick;
    RobotDrive drive;
    
    Victor armVictor;
    Victor leftMotorVictor;
    Victor rightMotorVictor;
    
    Servo triggerServo;
    
    public RobotTemplate() {
        
        //ds = DriverStation.getInstance();
        leftStick = new Joystick(1);
        rightStick = new Joystick(2);
        armVictor = new Victor(2);
        leftMotorVictor = new Victor(1);
        rightMotorVictor = new Victor(3); /* don't really know yet */
        drive = new RobotDrive(leftMotorVictor, rightMotorVictor);
        triggerServo = new Servo(5);
    }
    
    public void robotInit() {

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        drive.tankDrive(leftStick, rightStick);
        
        while (leftStick.getRawButton(2) && rightStick.getRawButton(2)) {
            armVictor.set(-0.5);
        }
        
        if (leftStick.getTrigger() && rightStick.getTrigger()) {
            triggerServo.set(0.15);
        }
    }
    
}
